#include "pid.h"
#include <string.h> // for memset

/**
 * @brief 初始化PID控制器
 */
void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd, float sample_time)
{
    // 清零整个结构体
    memset(pid, 0, sizeof(PID_Controller));

    // 设置增益和采样时间
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->sample_time = sample_time;

    // 设置默认输出限制
    pid->output_min = -1.0f; // 默认值，表示无限制
    pid->output_max = 1.0f;  // 默认值，表示无限制

    // 重置状态变量
    PID_Reset(pid);
    
    pid->initialized = true;
}

/**
 * @brief 更新PID计算
 */
float PID_Update(PID_Controller *pid, float setpoint, float measurement)
{
    if (!pid->initialized) {
        return 0.0f;
    }

    pid->setpoint = setpoint;
    float error = pid->setpoint - measurement;

    // 比例项
    float p_term = pid->Kp * error;

    // 积分项 (带抗饱和)
    pid->integral += error * pid->sample_time;
    if (pid->output_min != pid->output_max) {
        if (pid->integral > pid->output_max) {
            pid->integral = pid->output_max;
        } else if (pid->integral < pid->output_min) {
            pid->integral = pid->output_min;
        }
    }
    float i_term = pid->Ki * pid->integral;

    // 微分项
    float derivative = (error - pid->prev_error) / pid->sample_time;
    float d_term = pid->Kd * derivative;

    // 计算总输出
    float output = p_term + i_term + d_term;

    // 应用输出限制
    if (pid->output_min != pid->output_max) {
        if (output > pid->output_max) {
            output = pid->output_max;
        } else if (output < pid->output_min) {
            output = pid->output_min;
        }
    }

    // 保存当前误差以备下次使用
    pid->prev_error = error;

    return output;
}

/**
 * @brief 设置PID增益参数
 */
void PID_SetTunings(PID_Controller *pid, float Kp, float Ki, float Kd)
{
    if (!pid->initialized) {
        return;
    }
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
}

/**
 * @brief 设置PID输出限制
 */
void PID_SetOutputLimits(PID_Controller *pid, float min, float max)
{
    if (!pid->initialized || min >= max) {
        return;
    }
    pid->output_min = min;
    pid->output_max = max;
    
    // 如果积分项超出新的限制，则进行调整
    if (pid->integral > pid->output_max) {
        pid->integral = pid->output_max;
    } else if (pid->integral < pid->output_min) {
        pid->integral = pid->output_min;
    }
}

/**
 * @brief 重置PID控制器状态
 */
void PID_Reset(PID_Controller *pid)
{
    if (!pid->initialized) {
        return;
    }
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->last_time = 0;
}
